clc;clear;close all;

% 6连杆机械臂的重力及摩檫力补偿力矩
%关节变量为q1,q2,q3,q4,q5,q6  dqi为速度，摩擦力需要

%r为质心位置
xc_sym = sym('xc_%d',[6, 1],'real');
yc_sym = sym('yc_%d',[6, 1],'real');
zc_sym = sym('zc_%d',[6, 1],'real');

r1=[xc_sym(1) yc_sym(1) zc_sym(1) 1]';
r2=[xc_sym(2) yc_sym(2) zc_sym(2) 1]';
r3=[xc_sym(3) yc_sym(3) zc_sym(3) 1]';
r4=[xc_sym(4) yc_sym(4) zc_sym(4) 1]';
r5=[xc_sym(5) yc_sym(5) zc_sym(5) 1]';
r6=[xc_sym(6) yc_sym(6) zc_sym(6) 1]';

m = sym('m_%d',[6, 1],'real');
%g=9.81重力加速度
gg=[0 0 -sym('gz', 'real') 0];
% gg=[-sym('gx', 'real') -sym('gy', 'real') -sym('gz', 'real') 0];

%标准DH参数，UR构型
a=sym('a_%d',[6, 1],'real');
d=sym('d_%d',[6, 1],'real');
alpha=[pi/2,0,0,pi/2,-pi/2,0]; 
q=sym('q_%d',[6, 1],'real');

%齐次变换矩阵
for i=1:6
T(:,:,i)=[cos(q(i)),-sin(q(i))*cos(alpha(i)),sin(q(i))*sin(alpha(i)),a(i)*cos(q(i));
        sin(q(i)),cos(q(i))*cos(alpha(i)),-cos(q(i))*sin(alpha(i)),a(i)*sin(q(i));
        0,sin(alpha(i)),cos(alpha(i)),d(i);
        0,0,0,1];
end

% 4连杆臂
T01=T(:,:,1);
T02=T01*T(:,:,2);
T03=T02*T(:,:,3);
T04=T03*T(:,:,4);

for (i=1: 4)
    for (j=1: 4)
        % t1
        u41(i, j) = jacobian(T04(i, j), q(1));
        u31(i, j) = jacobian(T03(i, j), q(1));
        u21(i, j) = jacobian(T02(i, j), q(1));
        u11(i, j) = jacobian(T01(i, j), q(1));
        
        % t2
        u42(i, j) = jacobian(T04(i, j), q(2));
        u32(i, j) = jacobian(T03(i, j), q(2));
        u22(i, j) = jacobian(T02(i, j), q(2));
        
        % t3
        u43(i, j) = jacobian(T04(i, j), q(3));
        u33(i, j) = jacobian(T03(i, j), q(3));
        
        % t4
        u44(i, j) = jacobian(T04(i, j), q(4));
    end
end

% tg4 = simplify( -m(4)*gg*u44*r4 ); % 1*(1,4)*(4,4)*(4,1)
% tg3 = simplify( -m(3)*gg*u33*r3 - m(4)*gg*u43*r4 );
tg2 = simplify( -m(2)*gg*u22*r2 - m(3)*gg*u32*r3 - m(4)*gg*u42*r4 );
% tg1 = simplify( -m(1)*gg*u11*r1 - m(2)*gg*u21*r2 - m(3)*gg*u31*r3 - m(4)*gg*u41*r4 );

